/*
*	Author: Jacob Dufault
*	Date: July 8, 2009
*
*	Desc: The integrator takes forces and properly applies them to the object.
*		  Basically, it takes force and mass, and then calculates acceleration
*/
#ifndef _INTEGRATOR_HPP_
#define _INTEGRATOR_HPP_

#include "../utils/Numerics.hpp"
#include "../utils/Quaternion.hpp"
#include <list>
using namespace std;

namespace klasPhysics
{


	/// Physics state.
	struct State
	{
		/// primary physics state

		Vector3D location;				///< the position of the cube center of mass in world coordinates (meters).
		Vector3D momentum;			    ///< the momentum of the cube in kilogram meters per second.
		Quaternion orientation;			///< the orientation of the cube represented by a unit quaternion.
		Vector3D angularMomentum;		///< angular momentum vector.

		// secondary state

		Vector3D velocity;				///< velocity in meters per second (calculated from momentum).
		Quaternion spin;				///< quaternion rate of change in orientation.
		Vector3D angularVelocity;		///< angular velocity (calculated from angularMomentum).
		//Matrix bodyToWorld;			///< body to world coordinates matrix.
		//Matrix worldToBody;			///< world to body coordinates matrix.

		/// constant state

		Real size,						///< length of the cube sides in meters.
			 mass,						///< mass of the cube in kilograms.
			 inverseMass,				///< inverse of the mass used to convert momentum to velocity.
			 inertiaTensor,				///< inertia tensor of the cube (i have simplified it to a single value due to the mass properties a cube).
			 inverseInertiaTensor;		///< inverse inertia tensor used to convert angular momentum to angular velocity.

		/// Recalculate secondary state values from primary values.

		void Recalculate()
		{
			velocity = momentum * inverseMass;
			angularVelocity = angularMomentum * inverseInertiaTensor;
			orientation.normalize();
			spin = 0.5 * Quaternion(0, angularVelocity.x, angularVelocity.y, angularVelocity.z) * orientation;
			/*Matrix translation;
			translation.translate(position);
			bodyToWorld = translation * orientation.matrix();
			worldToBody = bodyToWorld.inverse();*/
		}
	};

	struct ForceList
	{
		list<Vector3D> directional,	// Forces that move the object
					   torque;		// Forces that spin the object

		void Clear()
		{
			directional.clear();
			torque.clear();
		}
	};

	/// Derivative values for primary state.
	/// This structure stores all derivative values for primary state in State.
	/// For example velocity is the derivative of position, force is the derivative
	/// of momentum etc. Storing all derivatives in this structure makes it easy
	/// to implement the RK4 integrator cleanly because it needs to calculate the
	/// and store derivative values at several points each timestep.

	struct Derivative
	{
		Vector3D velocity;                ///< velocity is the derivative of position.
		Vector3D force;                   ///< force in the derivative of momentum.
		Quaternion spin;                ///< spin is the derivative of the orientation quaternion.
		Vector3D torque;                  ///< torque is the derivative of angular momentum.
	};	




	class Integrator
	{
	private:
		static void ApplyObjectForces(Derivative &output, ForceList *forces);
		static Derivative Evaluate(const State &state, Real t, ForceList *forces);
		static Derivative Evaluate(State state, Real t, Real dt, const Derivative &derivative, ForceList *forces);

	public:
		static void Integrate(State &state, Real t, Real dt, ForceList *forces);

	};

}


#endif
